#include "Utils.h"

#include <cmath>
#include <iostream>
#include <filesystem>
#include <algorithm>



namespace fs = std::filesystem;

constexpr int MAX_ITERATIONS = 1000; // RANSAC算法的最大迭代次数
// 生成随机数
static double generateRandomNumber(double min, double max)
{
    return min + static_cast<double>(std::rand()) / (RAND_MAX / (max - min));
}

static double distance(Point p1, Point p2)
{
    double dx = p2.x - p1.x;
    double dy = p2.y - p1.y;
    return std::sqrt(dx * dx + dy * dy);
};

std::string Utils::TimeStamp2DateTimeString(double timestamp)
{
    time_t seconds = static_cast<time_t>(timestamp);
    double fractional = timestamp - seconds; // 划分整数秒和小数秒

    struct tm timeinfo;
#ifdef _WIN32
    localtime_s(&timeinfo, &seconds);
#else
    localtime_r(&seconds, &timeinfo);
#endif

    char buffer[80];
    strftime(buffer, sizeof(buffer), "%Y%m%d%H%M%S", &timeinfo);

    std::stringstream ss;
    ss << buffer
       << std::setw(3)
       << std::setfill('0')
       << static_cast<int>(fractional * 1000);

    return ss.str();
}

double Utils::DateTimeString2TimeStamp(std::string &s)
{
    // 验证输入格式
    if (s.length() != 17 || s.find_first_not_of("0123456789") != std::string::npos)
    {
        throw std::invalid_argument("输入格式必须为17位纯数字");
    }

    // 分解日期和毫秒部分
    const std::string datetime_str = s.substr(0, 14);
    const std::string millis_str = s.substr(14, 3);

    // 解析日期时间
    struct tm tm = {};
    std::istringstream ss(datetime_str);
    ss >> std::get_time(&tm, "%Y%m%d%H%M%S");

    if (ss.fail())
    {
        throw std::invalid_argument("日期时间解析失败");
    }

    // 转换为秒数（自动处理夏令时）
    const time_t seconds = mktime(&tm);
    if (seconds == -1)
    {
        throw std::invalid_argument("无效的日期时间值");
    }

    // 解析毫秒部分
    const int millis = std::stoi(millis_str);
    if (millis < 0 || millis > 999)
    {
        throw std::invalid_argument("毫秒值需在000-999之间");
    }

    // 合并为最终时间戳
    return static_cast<double>(seconds) + millis / 1000.0;
}

std::vector<std::string> Utils::FindRelevantFiles(const std::string &dir_path, double start_time_stamp, double end_time_stamp)
{
    std::vector<std::pair<uint64_t, std::string>> matched_files;
    for (const auto &entry : fs::directory_iterator(dir_path))
    {
        if (entry.path().extension() != ".stream")
            continue;

        std::string filename = entry.path().stem().string();
        size_t split_pos = filename.find("-");
        if (split_pos == std::string::npos)
            continue;

        std::string file_start_str = filename.substr(0, split_pos);
        std::string file_end_str = filename.substr(split_pos + 1);
        try
        {
            double file_start_time = DateTimeString2TimeStamp(file_start_str);
            double file_end_time = DateTimeString2TimeStamp(file_end_str);
            if (file_end_time >= start_time_stamp && file_start_time <= end_time_stamp)
                matched_files.emplace_back(file_start_time, entry.path().string());
        }
        catch (const std::invalid_argument &e)
        {
            continue;
        }
    }

    std::sort(matched_files.begin(), matched_files.end(), [](const auto &a, const auto &b)
              { return a.first < b.first; });

    std::vector<std::string> result;
    for (const auto &p : matched_files)
    {
        result.push_back(p.second);
    }

    return result;
}

double Utils::Rad(double d)
{
    return d * M_PI / 180.0;
}

Line Utils::Ransac(const std::vector<Point> &points, double torerance, int min_points)
{
    std::srand(std::time(nullptr)); // 初始化随机种子

    Line bestLine{0, 0};
    int bestInliers = 0;
    double bestDis = (std::numeric_limits<double>::max)();

    for (int i = 0; i < MAX_ITERATIONS; ++i)
    {
        // 随机选择两个点-必须在邻域找点
        int index1 = std::rand() % points.size();
        // int index2 = std::rand() % points.size();
        // while(index1 == index2)
        //     index2 = std::rand() % points.size();
        int offset = std::rand() % min_points + 1;
        while (index1 - offset < 0 && index1 + offset >= points.size())
        {
            offset -= 1;
        }
        int index2 = index1;
        if (index1 + offset >= points.size())
            index2 = index1 - offset;
        else
            index2 = index1 + offset;
        const Point &p1 = points[index1];
        const Point &p2 = points[index2];

        // 计算直线参数
        double slope = (p2.y - p1.y) / (p2.x - p1.x);
        double intercept = p1.y - slope * p1.x;

        // 统计内点数
        int inliers = 0;
        double dis = 0;
        int skip = -1;
        for (const Point &p : points)
        {
            double d = std::abs(p.y - slope * p.x - intercept) / std::sqrt(slope * slope + 1);
            if (d < torerance)
            {
                ++inliers;
                dis += d;
                skip = 0;
            }
            else
            {
                if (skip != -1)
                    skip++;
                if (skip > min_points)
                    break;
            }
        }
        dis = dis / inliers;

        // 更新最优直线
        if ((inliers > bestInliers && inliers >= min_points) || (inliers == bestInliers && inliers >= min_points && dis < bestDis))
        {
            bestInliers = inliers;
            bestLine.slope = slope;
            bestLine.intercept = intercept;
            bestDis = dis;
        }
    }

    return bestLine;
}

Point Utils::CalculateIntersection(const Line &line1, const Line &line2)
{
    const double epsilon = 1e-10; // 浮点精度容差
    Point p;

    // 处理斜率接近无穷大的特殊情况（可选）
    // 这里假设line结构体已能处理垂直情况（实际需要特殊处理）

    // 检查直线是否平行
    if (std::abs(line1.slope - line2.slope) < epsilon)
    {
        p.x = 0;
        p.y = 0;
        return p; // 平行或重合，无唯一交点
    }

    // 计算交点坐标
    p.x = (line2.intercept - line1.intercept) / (line1.slope - line2.slope);
    p.y = line1.slope * p.x + line1.intercept;

    // 验证交点是否在两条直线上（可选）
    double delta = std::abs(line2.slope * p.x + line2.intercept - p.y);
    return p;
}

bool Utils::IsPointOnRay(Point p, Point origin, Point direct)
{
    const double epsilon = 1e-10; // 浮点精度容差

    // 处理原点与方向点重合的情况
    if (std::abs(origin.x - direct.x) < epsilon &&
        std::abs(origin.y - direct.y) < epsilon)
    {
        return (std::abs(p.x - origin.x) < epsilon &&
                std::abs(p.y - origin.y) < epsilon);
    }

    // 计算方向向量
    double dx = direct.x - origin.x;
    double dy = direct.y - origin.y;

    // 处理垂直方向（x坐标相同）
    if (std::abs(dx) < epsilon)
    {
        // 检查x坐标是否相同，且y方向是否一致
        return std::abs(p.x - origin.x) < epsilon &&
               ((dy > 0 && p.y >= origin.y) ||
                (dy < 0 && p.y <= origin.y));
    }

    // 处理水平方向（y坐标相同）
    if (std::abs(dy) < epsilon)
    {
        // 检查y坐标是否相同，且x方向是否一致
        return std::abs(p.y - origin.y) < epsilon &&
               ((dx > 0 && p.x >= origin.x) ||
                (dx < 0 && p.x <= origin.x));
    }

    // 通用情况：计算参数t（需满足t >= 0）
    double tx = (p.x - origin.x) / dx;
    double ty = (p.y - origin.y) / dy;

    // 检查参数t是否相等且在非负区间
    return (std::abs(tx - ty) < epsilon) && (tx >= 0 - epsilon);
}

std::vector<Segment> Utils::RansacPoly(const std::vector<Point> &points, double torerance, int min_points)
{
    std::vector<Segment> base_segment;
    if (points.size() < min_points)
        return base_segment;

    std::vector<Point> prev_points; // 前置点集
    std::vector<Point> next_points; // 后置点集

    // 拟合当前点集
    Line line = Ransac(points, torerance, min_points);
    // std::cout << "RansacPoly: " << points.size() << ", " << line.slope << ", " << line.intercept << std::endl;
    bool isOver = false;
    bool isStart = false;
    Point start_point;
    Point end_point;
    std::vector<Point> temp_points;
    for (const Point &p : points)
    {
        if (isOver)
        {
            next_points.push_back(p);
            continue;
        }

        double d = std::abs(p.y - line.slope * p.x - line.intercept) / std::sqrt(line.slope * line.slope + 1);

        if (d > torerance)
        {
            temp_points.push_back(p);
            if (isStart && temp_points.size() > min_points) // 在开始后连续不属于直线，则放入后置直线，该条线段结束
            {
                for (int i = 0; i < temp_points.size(); i++)
                {
                    next_points.push_back(temp_points[i]);
                }
                isOver = true;
                temp_points.clear();
            }
        }
        else
        {
            if (!isStart)
            {
                for (int i = 0; i < temp_points.size(); i++) // 第一个点找到，放入前置点集
                {
                    prev_points.push_back(temp_points[i]);
                }
                temp_points.clear();
                start_point = p; // 赋值初始值
            }
            isStart = true;
            temp_points.clear(); // 中间断点去除
            end_point = p;
        }
    }
    for (int i = 0; i < temp_points.size(); i++) // 处理剩余点
    {
        next_points.push_back(temp_points[i]);
    }

    // std::cout << points.size() << "," << start_point.index << "," << end_point.index << "," << prev_points.size() << "," << next_points.size() << std::endl;
    Segment segment{line, start_point, end_point};
    std::vector<Segment> prev_segments = RansacPoly(prev_points, torerance, min_points);
    std::vector<Segment> next_segments = RansacPoly(next_points, torerance, min_points);
    for (int i = 0; i < prev_segments.size(); i++)
    {
        if (prev_segments[i].line.intercept != 0 || prev_segments[i].line.slope != 0)
            base_segment.push_back(prev_segments[i]);
    }
    if (segment.line.intercept != 0 || segment.line.slope != 0)
        base_segment.push_back(segment);
    for (int i = 0; i < next_segments.size(); i++)
    {
        if (next_segments[i].line.intercept != 0 || next_segments[i].line.slope != 0)
            base_segment.push_back(next_segments[i]);
    }

    return base_segment;
}

void Utils::printBinary(uint64_t value)
{
    // 方法1：使用bitset直接转换（显示64位）
    std::cout << "二进制: " << std::bitset<64>(value) << std::endl;

    // 方法2：按字节打印（更易观察字节序）
    const uint8_t *bytes = reinterpret_cast<const uint8_t *>(&value);
    std::cout << "字节序: ";
    for (int i = 0; i < 8; ++i)
    {
        std::cout << std::bitset<8>(bytes[i]) << " ";
    }
    std::cout << "\n十六进制: 0x" << std::hex << std::setw(16) << std::setfill('0') << value << std::dec << std::endl;
}